import pybullet as p
from time import sleep

#world
world_id = p.connect(p.GUI)
p.setGravity(0,0,-9.8)

#plane
plane_id = p.createCollisionShape(p.GEOM_PLANE)
p.createMultiBody(0, plane_id)

#boxes
box_collision_id = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.5, 0.5, 0.5])
box_visual_id = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.5, 0.5, 0.5], rgbaColor=[1, 0, 0, 1] )

for i in range(3):
    box_id = p.createMultiBody(
        baseMass=i * 3 + 1,
        baseCollisionShapeIndex=box_collision_id,  # 重用碰撞形状
        baseVisualShapeIndex=box_visual_id,        # 重用视觉形状
        basePosition=[i*2, 0, 5]               # 每个实例有不同的位置)

        )
try:
    while True:
        p.stepSimulation()
        pos, _ = p.getBasePositionAndOrientation(1)
        print(f"Box position: {pos}")
        sleep(1/100)

except KeyboardInterrupt:
    print("Quit!")
    pass

finally:
    p.disconnect()